/** ###################################################################
**     Filename  : lcd_gui.c
**     Project   : ccd2
**     Processor : MCF52255CAF80
**     Version   : Driver 01.00
**     Compiler  : CodeWarrior MCF C Compiler
**     Arthor	 : Yumi Chan, Cindy Yu, Ryan Cheung
**     Abstract  :
**         
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/


/* Include */
#include "lcd_gui.h"
#include "LED.h"


/* Private Methods */
void LCD_GUI(GUI_PAGE page)
{
	uint8 item = 0;
	int tmp = 0;
	int i = 0;
	char str[6] = { 0 };
	bool isLeave = FALSE;
	
	LcdClear(0x0000);

	do 
	{
		if (!(joystick_up_GetVal()))
		{
			break;
		}
		
		switch (page)
		{
		case MENU:
			{
				//LcdDrawSquare(0, 0, 128, LINE_HEIGHT, 0x00F0);
				LcdDrawString(0, LINE_HEIGHT*0, "HKUST CCDII 2014", 0xFFFF, 0x00F0);
				
				while(1)
				{
					if (!(joystick_down_GetVal()))
					{
						if (item == 2)
						{
							item = 0;
						}
						else
						{
							++item;
						}
					}
					else if (!(joystick_left_GetVal() && joystick_up_GetVal()))
					{
						isLeave = TRUE;
						break;
					}
					else if (!(joystick_centre_GetVal()))
					{
						page = item + 1;
						break;
					}
					
					GUI_PrintMenu(item);
					Cpu_Delay100US(50);
				}
			
			}
			break;
		
		case CCD_SETTING:
			{
				LcdDrawSquare(0, 0, UNUSED_PIXEL_1SIDE, 8, 0xF0FF);
				LcdDrawSquare(CCD_RESOLUTION-UNUSED_PIXEL_1SIDE, 0, UNUSED_PIXEL_1SIDE, 8, 0xF0FF);
				LcdDrawString(0, LINE_HEIGHT*1, "CCD1 TH:\0", 0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*2, "edges:\0", 0xFFFF,0x0000);
				LcdDrawString(0, LINE_HEIGHT*3, "center:\0", 0xFFFF,0x0000);
				LcdDrawString(0, LINE_HEIGHT*4, "width:\0", 0xFFFF,0x0000);
#if (USE_TWO_LINEAR_CCD)
				LcdDrawSquare(0, 9, UNUSED_PIXEL_1SIDE, 8, 0xF0FF);
				LcdDrawSquare(CCD_RESOLUTION-UNUSED_PIXEL_1SIDE, 9, UNUSED_PIXEL_1SIDE, 8, 0xF0FF);
				LcdDrawString(0, LINE_HEIGHT*6, "CCD2 TH:\0", 0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*7, "edges:\0", 0xFFFF,0x0000);
				LcdDrawString(0, LINE_HEIGHT*8, "center:\0", 0xFFFF,0x0000);
				LcdDrawString(0, LINE_HEIGHT*9, "width:\0", 0xFFFF,0x0000);
#endif
				while(1)
				{
					if (!(joystick_down_GetVal()))
					{
						if (item == 1)
						{
							item = 0;
						}
						else
						{
							++item;
						}
					}
					else if (!(joystick_up_GetVal()))
					{
						page = MENU;
						break;
					}
					else if (!(joystick_left_GetVal()))
					{
						if (ccd[item].threshold >= 10)
						{
							ccd[item].threshold -= 10;
							if (item == CCD1)
							{
								CCD_threshold_SetDutyUS(ccd[CCD1].threshold);
							}
							else if (item == CCD2)
							{
								CCD2_threshold_SetDutyUS(ccd[CCD2].threshold);
							}
							
						}
					}
					else if (!(joystick_right_GetVal()))
					{
						if (ccd[item].threshold <= 4990)
						{
							ccd[item].threshold += 10;
							if (item == CCD1)
							{
								CCD_threshold_SetDutyUS(ccd[CCD1].threshold);
							}
							else if (item == CCD2)
							{
								CCD2_threshold_SetDutyUS(ccd[CCD2].threshold);
							}
						}
					}
					
  					/* Collect CCD image 3 times */
  					if (time_ms >= 8)
  					{
  						time_ms = 0;
  					}
  					
  					for (i=0; i<3; )
 					{
  						if (time_ms == 8)
  						{
  							time_ms = 0;
  							++i;
    						collectCCDImage();
  						}
  					}
					
					findTrackWidth();
					
					GUI_PrintCCDSetting(item);
					
  	  	  		/*for (i=0; i<IMAGE_RESOLUTION; ++i)
  	  	  		{
  	  	  	  		if(ccd[CCD1].image[i] == 0xFF)
  	  	  	 	 	{
  	  	  	  			UART_SendChar(0xFE);
  	  	  	 			delay_10us(1);
  	  	  	  			continue;
  	  	  	 		}
  	  	  	  		UART_SendChar(ccd[CCD1].image[i]);
  	  	  	  		delay_10us(1);
  	  	  		 }
  	  	  		 UART_SendChar(0xFF);*/
				}
				
			}
			break;
		
		case SPEED_PID:
			{
				LcdDrawString(0, LINE_HEIGHT*0, "Speed PID:\0" ,0xF0FF,0xFFFF);
				LcdDrawString(0, LINE_HEIGHT*1, "Kp:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*2, "Ki:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*3, "Kd:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*4, "Set:\0" ,0xFFF0,0x0000);
				
				tmp = abs(speedPID.constant[Kp]);
				intToString(tmp, str, 10);
				LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFF0, 0xF000);
				clearString(str, 6);
			
				tmp = abs(speedPID.constant[Ki]);
				intToString(tmp, str, 10);
				LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
				clearString(str, 6);
			
				tmp = abs(speedPID.constant[Kd]);
				intToString(tmp, str, 10);
				LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
				clearString(str, 6);
				
				intToString(speedPID.set_value, str, 10);
				LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
				clearString(str, 6);
				
				while(1)
				{
					if (!(joystick_down_GetVal()))
					{
						switch(item)
						{
						case 0:
							LcdDrawSquare(50, LINE_HEIGHT*1, 56, LINE_HEIGHT, 0x0000);
							tmp = abs(speedPID.constant[Kp]);
							intToString(tmp, str, 10);
							LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 1:
							LcdDrawSquare(50, LINE_HEIGHT*2, 56, LINE_HEIGHT, 0x0000);
							tmp = abs(speedPID.constant[Ki]);
							intToString(tmp, str, 10);
							LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 2:
							LcdDrawSquare(50, LINE_HEIGHT*3, 56, LINE_HEIGHT, 0x0000);
							tmp = abs(speedPID.constant[Kd]);
							intToString(tmp, str, 10);
							LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 3:
							LcdDrawSquare(50, LINE_HEIGHT*4, 56, LINE_HEIGHT, 0x0000);
							intToString(speedPID.set_value, str, 10);
							LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
						}
						
						if (item == 3)
						{
							item = 0;
						}
						else
						{
							++item;
						}
						Cpu_Delay100US(300);
					}
					else if (!(joystick_up_GetVal()))
					{
						page = MENU;
						break;
					}
					else if (!(joystick_left_GetVal()))
					{
						switch (item)
						{
						case 0:
							if (speedPID.constant[Kp] < 0)
							{
								++speedPID.constant[Kp];
							}
							break;
							
						case 1:
							if (speedPID.constant[Ki] < 0)
							{
								++speedPID.constant[Ki];
							}
							break;
							
						case 2:
							if (speedPID.constant[Kd] < 0)
							{
								++speedPID.constant[Kd];
							}
							break;
							
						case 3:
							if (speedPID.set_value > 0)
							{
								speedPID.set_value -= 10;
							}
							break;
						}
					}
					else if (!(joystick_right_GetVal()))
					{
						switch (item)
						{
						case 0:
							--speedPID.constant[Kp];
							break;
							
						case 1:
							--speedPID.constant[Ki];
							break;
							
						case 2:
							--speedPID.constant[Kd];
							break;
							
						case 3:
							speedPID.set_value += 10;
							break;
						}
					}
	
					GUI_PrintSpeedPID(item);
					Cpu_Delay100US(50);
				}
			}
			break;
			
		case SERVO_PID:
			{
				LcdDrawString(0, LINE_HEIGHT*0, "Servo PID:\0" ,0xF0FF,0xFFFF);
				LcdDrawString(0, LINE_HEIGHT*1, "Kp:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*2, "a:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*3, "b:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*4, "Kp_smER:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*5, "ER_th:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*6, "Ki:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*7, "Kd:\0" ,0xFFF0,0x0000);
				LcdDrawString(0, LINE_HEIGHT*8, "Set:\0" ,0xFFFF,0x0000);
				LcdDrawString(0, LINE_HEIGHT*9, "st Road:\0" ,0xFFF0,0x0000);
				
				intToString(servoPID.constant[Kp], str, 10);
				LcdDrawString(65, LINE_HEIGHT*1, str, 0xFFF0, 0x0000);
				clearString(str, 6);
	
				tmp = a * 10000;
				intToString(tmp, str, 10);
				LcdDrawString(65, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
				clearString(str, 6);
	
				tmp = b * 10;
				intToString(tmp, str, 10);
				LcdDrawString(65, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
				clearString(str, 6);
		
				tmp = kp_for_small_error * 10;
				intToString(tmp, str, 10);
				LcdDrawString(65, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
				clearString(str, 6);
		
				intToString(error_threshold, str, 10);
				LcdDrawString(65, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
				clearString(str, 6);
	
				intToString(servoPID.constant[Ki], str, 10);
				LcdDrawString(65, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
				clearString(str, 6);
			
				intToString(servoPID.constant[Kd], str, 10);
				LcdDrawString(65, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
				clearString(str, 6);
				
				intToString(servoPID.set_value, str, 10);
				LcdDrawString(65, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
				clearString(str, 6);
				
				intToString(straightRoad_threshold, str, 10);
				LcdDrawString(65, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
				clearString(str, 6);
				
				while(1)
				{
					if (!(joystick_down_GetVal()))
					{
						switch (item)
						{
						case 0:
							LcdDrawSquare(65, LINE_HEIGHT*1, 48, LINE_HEIGHT, 0x0000);
							intToString(servoPID.constant[Kp], str, 10);
							LcdDrawString(65, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 1:
							LcdDrawSquare(65, LINE_HEIGHT*2, 48, LINE_HEIGHT, 0x0000);
							tmp = a * 10000;
							intToString(tmp, str, 10);
							LcdDrawString(65, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 2:
							LcdDrawSquare(65, LINE_HEIGHT*3, 48, LINE_HEIGHT, 0x0000);
							tmp = b * 10;
							intToString(tmp, str, 10);
							LcdDrawString(65, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 3:
							LcdDrawSquare(65, LINE_HEIGHT*4, 48, LINE_HEIGHT, 0x0000);
							tmp = kp_for_small_error * 10;
							intToString(tmp, str, 10);
							LcdDrawString(65, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 4:
							LcdDrawSquare(65, LINE_HEIGHT*5, 48, LINE_HEIGHT, 0x0000);
							intToString(error_threshold, str, 10);
							LcdDrawString(65, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 5:
							LcdDrawSquare(65, LINE_HEIGHT*6, 48, LINE_HEIGHT, 0x0000);
							intToString(servoPID.constant[Ki], str, 10);
							LcdDrawString(65, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 6:
							LcdDrawSquare(65, LINE_HEIGHT*7, 48, LINE_HEIGHT, 0x0000);
							intToString(servoPID.constant[Kd], str, 10);
							LcdDrawString(65, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
							
						case 7:
							LcdDrawSquare(65, LINE_HEIGHT*9, 48, LINE_HEIGHT, 0x0000);
							intToString(straightRoad_threshold, str, 10);
							LcdDrawString(65, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
							clearString(str, 6);
							break;
						}
						if (item == 7)
						{
							item = 0;
						}
						else
						{
							++item;
						}
						
						Cpu_Delay100US(300);
					}
					else if (!(joystick_up_GetVal()))
					{
						page = MENU;
						break;
					}
					else if (!(joystick_left_GetVal()))
					{
						switch (item)
						{
						case 0:
							if (servoPID.constant[Kp] > 0)
							{
								--servoPID.constant[Kp];
							}
							break;
							
						case 1:
							if (a > 0)
							{
								a -= 0.0001;
							}
							break;
							
						case 2:
							if (b > 0)
							{
								b -= 0.1;
							}
							break;
							
						case 3:
							if (kp_for_small_error > 0)
							{
								kp_for_small_error -= 0.1;
							}
							break;
							
						case 4:
							if (error_threshold > 0)
							{
								--error_threshold;
							}
							break;
							
						case 5:
							if (servoPID.constant[Ki] > 0)
							{
								--servoPID.constant[Ki];
							}
							break;
							
						case 6:
							if (servoPID.constant[Kd] > 0)
							{
								--servoPID.constant[Kd];
							}
							break;
							
						case 7:
							if (straightRoad_threshold > 0)
							{
								--straightRoad_threshold;
							}
							break;
						}
					}
					else if (!(joystick_right_GetVal()))
					{
						switch (item)
						{
						case 0:
							++servoPID.constant[Kp];
							break;
							
						case 1:
							a += 0.0001;
							break;
							
						case 2:
							b += 0.1;
							break;
							
						case 3:
							kp_for_small_error += 0.1;
							break;
							
						case 4:
							++error_threshold;
							break;
							
						case 5:
							++servoPID.constant[Ki];
							break;
							
						case 6:
							++servoPID.constant[Kd];
							break;
							
						case 7:
							++straightRoad_threshold;
							break;
						}
					}
					
					GUI_PrintServoPID(item);
					Cpu_Delay100US(50);
				}
			
			}
			break;
		}
		
		item = 0;
		LcdClear(0x0000);
		Cpu_Delay100US(200);
	} while (!isLeave);
  	
  	return ;
}


void intToString(int num, char* str, uint8_t index)
{
	uint8_t digit_count = 1;
	uint8_t digit_count_img;
	uint8_t digit[16];
	int value = num;
	int scale = index;
	int i;
	
	if(value < 0){
		value = abs(num);
	}
	
	while(value >= scale){
		scale *= index;
		digit_count++;
	}
	
	for(i = digit_count - 1; i >= 0 ; i--){
		scale /= index;
		digit[i] = value / scale;
		value -= digit[i] * scale;
		
	}

	digit_count_img = digit_count;
	
	if(num < 0){
		str[0] = '-';
		digit_count++;
	}
	
	for(i = digit_count_img - 1; i >= 0; i--){
		if(digit[i] > 9){
			str[digit_count - 1 - i] = 'A' + digit[i] - 10;
		}
		else{
			str[digit_count - 1 - i] = '0' + digit[i];
		}
	}
}

void clearString(char* str, uint32 size)
{
	int i = 0;
	for(; i < size; ++i)
	{
		str[i] = 0;
	}
	//str[size-1] = 0;
	
	return ;
}

void GUI_PrintMenu(uint8 selected)
{
	switch (selected)
	{
	case 0:
		LcdDrawString(0, LINE_HEIGHT*1, "CCD Setting\0", 0xF0FF,0xFFFF);
		LcdDrawString(0, LINE_HEIGHT*2, "Speed PID\0", 0xFFFF,0x0000);
		LcdDrawString(0, LINE_HEIGHT*3, "Servo PID\0", 0xFFFF,0x0000);
		break;
		
	case 1:
		LcdDrawString(0, LINE_HEIGHT*1, "CCD Setting\0", 0xFFFF,0x0000);
		LcdDrawString(0, LINE_HEIGHT*2, "Speed PID\0", 0xF0FF,0xFFFF);
		LcdDrawString(0, LINE_HEIGHT*3, "Servo PID\0", 0xFFFF,0x0000);
		break;
		
	case 2:
		LcdDrawString(0, LINE_HEIGHT*1, "CCD Setting\0", 0xFFFF,0x0000);
		LcdDrawString(0, LINE_HEIGHT*2, "Speed PID\0", 0xFFFF,0x0000);
		LcdDrawString(0, LINE_HEIGHT*3, "Servo PID\0", 0xF0FF,0xFFFF);
		break;
	}
	
	return ;
}

void GUI_PrintCCDSetting(uint8 selected)
{
	short i = 0;
	char str_thre[5] =  { 0 };
	char str[4] =  { 0 };
	
	switch (selected)
	{
	case 0:
		LcdDrawSquare(73, LINE_HEIGHT*1, 34, LINE_HEIGHT, 0xF000);
		intToString(ccd[CCD1].threshold, str_thre, 10);
		LcdDrawString(75, LINE_HEIGHT*1, str_thre, 0xFFF0,0xF000);
		clearString(str_thre, 5);
		
		for(i=0; i < IMAGE_RESOLUTION; ++i)
		{
			if(ccd[CCD1].image[i] == BLACK)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 0, 1, 6, 0x0000);
			}
			else if (ccd[CCD1].image[i] == WHITE)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 0, 1, 6, 0xFFFF);
			}
		}
		
		LcdDrawSquare(65, LINE_HEIGHT*2, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].left_edge_pos[CURRENT], str, 10);
		LcdDrawString(65, LINE_HEIGHT*2, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(95, LINE_HEIGHT*2, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].right_edge_pos[CURRENT], str, 10);
		LcdDrawString(95, LINE_HEIGHT*2, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(80, LINE_HEIGHT*3, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].trackCenter, str, 10);
		LcdDrawString(80, LINE_HEIGHT*3, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(65, LINE_HEIGHT*4, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].trackWidth, str, 10);
		LcdDrawString(65, LINE_HEIGHT*4, str, 0xFFFF,0x0000);
		clearString(str, 4);

#if (USE_TWO_LINEAR_CCD)
		LcdDrawSquare(73, LINE_HEIGHT*6, 34, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].threshold, str_thre, 10);
		LcdDrawString(75, LINE_HEIGHT*6, str_thre, 0xFFFF,0x0000);
		clearString(str_thre, 5);
		
		for(i=0; i < IMAGE_RESOLUTION; ++i)
		{
			if(ccd[CCD2].image[i] == BLACK)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 7, 1, 6, 0x0000);
			}
			else if (ccd[CCD2].image[i] == WHITE)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 7, 1, 6, 0xFFFF);
			}
		}
		
		LcdDrawSquare(65, LINE_HEIGHT*7, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].left_edge_pos[CURRENT], str, 10);
		LcdDrawString(65, LINE_HEIGHT*7, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(95, LINE_HEIGHT*7, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].right_edge_pos[CURRENT], str, 10);
		LcdDrawString(95, LINE_HEIGHT*7, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(80, LINE_HEIGHT*8, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].trackCenter, str, 10);
		LcdDrawString(80, LINE_HEIGHT*8, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(65, LINE_HEIGHT*9, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].trackWidth, str, 10);
		LcdDrawString(65, LINE_HEIGHT*9, str, 0xFFFF,0x0000);
		clearString(str, 4);
#endif
		
		break;
		
	case 1:
		LcdDrawSquare(73, LINE_HEIGHT*1, 34, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].threshold, str_thre, 10);
		LcdDrawString(75, LINE_HEIGHT*1, str_thre, 0xFFFF,0x0000);
		clearString(str_thre, 5);
		
		for(i=0; i < IMAGE_RESOLUTION; ++i)
		{
			if(ccd[CCD1].image[i] == BLACK)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 0, 1, 6, 0x0000);
			}
			else if (ccd[CCD1].image[i] == WHITE)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 0, 1, 6, 0xFFFF);
			}
		}
		
		LcdDrawSquare(65, LINE_HEIGHT*2, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].left_edge_pos[CURRENT], str, 10);
		LcdDrawString(65, LINE_HEIGHT*2, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(95, LINE_HEIGHT*2, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].right_edge_pos[CURRENT], str, 10);
		LcdDrawString(95, LINE_HEIGHT*2, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(80, LINE_HEIGHT*3, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].trackCenter, str, 10);
		LcdDrawString(80, LINE_HEIGHT*3, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(65, LINE_HEIGHT*4, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD1].trackWidth, str, 10);
		LcdDrawString(65, LINE_HEIGHT*4, str, 0xFFFF,0x0000);
		clearString(str, 4);


#if (USE_TWO_LINEAR_CCD)
		LcdDrawSquare(73, LINE_HEIGHT*6, 34, LINE_HEIGHT, 0xF000);
		intToString(ccd[CCD2].threshold, str_thre, 10);
		LcdDrawString(75, LINE_HEIGHT*6, str_thre, 0xFFF0,0xF000);
		clearString(str_thre, 5);
		
		for(i=0; i < IMAGE_RESOLUTION; ++i)
		{
			if(ccd[CCD2].image[i] == BLACK)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 7, 1, 6, 0x0000);
			}
			else if (ccd[CCD2].image[i] == WHITE)
			{
				LcdDrawSquare(i+UNUSED_PIXEL_1SIDE, 7, 1, 6, 0xFFFF);
			}
		}
		
		LcdDrawSquare(65, LINE_HEIGHT*7, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].left_edge_pos[CURRENT], str, 10);
		LcdDrawString(65, LINE_HEIGHT*7, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(95, LINE_HEIGHT*7, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].right_edge_pos[CURRENT], str, 10);
		LcdDrawString(95, LINE_HEIGHT*7, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(80, LINE_HEIGHT*8, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].trackCenter, str, 10);
		LcdDrawString(80, LINE_HEIGHT*8, str, 0xFFFF,0x0000);
		clearString(str, 4);
		
		LcdDrawSquare(65, LINE_HEIGHT*9, 32, LINE_HEIGHT, 0x0000);
		intToString(ccd[CCD2].trackWidth, str, 10);
		LcdDrawString(65, LINE_HEIGHT*9, str, 0xFFFF,0x0000);
		clearString(str, 4);
#endif
		break;
	}
	
	return ;
}

void GUI_PrintSpeedPID(uint8 selected) 
{
	int tmp = 0;
	//char str[6] = { 32, 32, 32, 32, 32, 0 };
	char str[6] = { 0 };
	
	switch (selected)
	{
	case 0:
		LcdDrawSquare(50, LINE_HEIGHT*1, 56, LINE_HEIGHT, 0xF000);
		tmp = abs(speedPID.constant[Kp]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*tmp = abs(speedPID.constant[Ki]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = abs(speedPID.constant[Kd]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(speedPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 1:
		/*tmp = abs(speedPID.constant[Kp]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
	
		LcdDrawSquare(50, LINE_HEIGHT*2, 56, LINE_HEIGHT, 0xF000);
		tmp = abs(speedPID.constant[Ki]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*tmp = abs(speedPID.constant[Kd]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(speedPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 2:
		/*tmp = abs(speedPID.constant[Kp]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = abs(speedPID.constant[Ki]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
	
		LcdDrawSquare(50, LINE_HEIGHT*3, 56, LINE_HEIGHT, 0xF000);
		tmp = abs(speedPID.constant[Kd]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		
		/*intToString(speedPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 3:
		/*tmp = abs(speedPID.constant[Kp]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = abs(speedPID.constant[Ki]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = abs(speedPID.constant[Kd]);
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		
		LcdDrawSquare(50, LINE_HEIGHT*4, 56, LINE_HEIGHT, 0xF000);
		intToString(speedPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		break;
	}
	
	return ;
}


void GUI_PrintServoPID(uint8 selected) 
{
	int tmp = 0;
	//char str[8] = { 32, 32, 32, 32, 32, 32, 32, 0 };
	char str[6] = { 0 };
	
	switch (selected)
	{
	case 0:
		LcdDrawSquare(65, LINE_HEIGHT*1, 48, LINE_HEIGHT, 0xF000);
		intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(65, LINE_HEIGHT*1, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 1:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
	
		LcdDrawSquare(65, LINE_HEIGHT*2, 48, LINE_HEIGHT, 0xF000);
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(65, LINE_HEIGHT*2, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 2:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
	
		LcdDrawSquare(65, LINE_HEIGHT*3, 48, LINE_HEIGHT, 0xF000);
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(65, LINE_HEIGHT*3, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		
		/*tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 3:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		
		LcdDrawSquare(65, LINE_HEIGHT*4, 48, LINE_HEIGHT, 0xF000);
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(65, LINE_HEIGHT*4, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		
		/*intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 4:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		
		LcdDrawSquare(65, LINE_HEIGHT*5, 48, LINE_HEIGHT, 0xF000);
		intToString(error_threshold, str, 10);
		LcdDrawString(65, LINE_HEIGHT*5, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 5:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
	
		LcdDrawSquare(65, LINE_HEIGHT*6, 48, LINE_HEIGHT, 0xF000);
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(65, LINE_HEIGHT*6, str, 0xFFF0, 0xF000);
		clearString(str, 6);
	
		/*intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 6:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		
		LcdDrawSquare(65, LINE_HEIGHT*7, 48, LINE_HEIGHT, 0xF000);
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(65, LINE_HEIGHT*7, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		
		/*intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*9, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		break;
		
	case 7:
		/*intToString(servoPID.constant[Kp], str, 10);
		LcdDrawString(50, LINE_HEIGHT*1, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = a * 10000;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*2, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		tmp = b * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*3, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		tmp = kp_for_small_error * 10;
		intToString(tmp, str, 10);
		LcdDrawString(50, LINE_HEIGHT*4, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(error_threshold, str, 10);
		LcdDrawString(50, LINE_HEIGHT*5, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Ki], str, 10);
		LcdDrawString(50, LINE_HEIGHT*6, str, 0xFFFF, 0x0000);
		clearString(str, 6);
	
		intToString(servoPID.constant[Kd], str, 10);
		LcdDrawString(50, LINE_HEIGHT*7, str, 0xFFFF, 0x0000);
		clearString(str, 6);
		
		intToString(servoPID.set_value, str, 10);
		LcdDrawString(50, LINE_HEIGHT*8, str, 0xFFFF, 0x0000);
		clearString(str, 6);*/
		
		LcdDrawSquare(65, LINE_HEIGHT*9, 48, LINE_HEIGHT, 0xF000);
		intToString(straightRoad_threshold, str, 10);
		LcdDrawString(65, LINE_HEIGHT*9, str, 0xFFF0, 0xF000);
		clearString(str, 6);
		break;
	}
	
	return ;
}


